#ifndef MATRIX_H
#define MATRIX_H

#include "Common/Output.h"
#include "Math.h"
#include "Vec3.h"

class Matrix
{
public:
	struct MatStruct {
		float	
			m_E11, m_E12, m_E13, m_E14,
			m_E21, m_E22, m_E23, m_E24,
			m_E31, m_E32, m_E33, m_E34,
			m_E41, m_E42, m_E43, m_E44;
	} ;

	union
	{
		struct MatStruct m_E;
		float mLineMat[16];
		float mMat[4][4];
	};

	Matrix()
	{
		SetIdentity();
	};

	Matrix(
		float E11, float E12, float E13, float E14,
		float E21, float E22, float E23, float E24,
		float E31, float E32, float E33, float E34,
		float E41, float E42, float E43, float E44)
	{
		m_E.m_E11 = E11; m_E.m_E12 = E12; m_E.m_E13 = E13; m_E.m_E14 = E14;
		m_E.m_E21 = E21; m_E.m_E22 = E22; m_E.m_E23 = E23; m_E.m_E24 = E24;
		m_E.m_E31 = E31; m_E.m_E32 = E32; m_E.m_E33 = E33; m_E.m_E34 = E34;
		m_E.m_E41 = E41; m_E.m_E42 = E42; m_E.m_E43 = E43; m_E.m_E44 = E44;
	};

	Matrix(const Matrix & M)
	{
		m_E.m_E11 = M.m_E.m_E11; m_E.m_E12 = M.m_E.m_E12; m_E.m_E13 = M.m_E.m_E13; m_E.m_E14 = M.m_E.m_E14;
		m_E.m_E21 = M.m_E.m_E21; m_E.m_E22 = M.m_E.m_E22; m_E.m_E23 = M.m_E.m_E23; m_E.m_E24 = M.m_E.m_E24;
		m_E.m_E31 = M.m_E.m_E31; m_E.m_E32 = M.m_E.m_E32; m_E.m_E33 = M.m_E.m_E33; m_E.m_E34 = M.m_E.m_E34;
		m_E.m_E41 = M.m_E.m_E41; m_E.m_E42 = M.m_E.m_E42; m_E.m_E43 = M.m_E.m_E43; m_E.m_E44 = M.m_E.m_E44;
	};

	void SetIdentity()
	{
		m_E.m_E11 = 1.0f; m_E.m_E12 = 0.0f; m_E.m_E13 = 0.0f; m_E.m_E14 = 0.0f;
		m_E.m_E21 = 0.0f; m_E.m_E22 = 1.0f; m_E.m_E23 = 0.0f; m_E.m_E24 = 0.0f;
		m_E.m_E31 = 0.0f; m_E.m_E32 = 0.0f; m_E.m_E33 = 1.0f; m_E.m_E34 = 0.0f;
		m_E.m_E41 = 0.0f; m_E.m_E42 = 0.0f; m_E.m_E43 = 0.0f; m_E.m_E44 = 1.0f;
	};

	inline Matrix * Clone(void);

	inline Matrix operator + (Matrix & M)
	{
		return(Matrix(
			m_E.m_E11+M.m_E.m_E11, m_E.m_E12+M.m_E.m_E12, m_E.m_E13+M.m_E.m_E13, m_E.m_E14+M.m_E.m_E14,
			m_E.m_E21+M.m_E.m_E21, m_E.m_E22+M.m_E.m_E22, m_E.m_E23+M.m_E.m_E23, m_E.m_E24+M.m_E.m_E24,
			m_E.m_E31+M.m_E.m_E31, m_E.m_E32+M.m_E.m_E32, m_E.m_E33+M.m_E.m_E33, m_E.m_E34+M.m_E.m_E34,
			m_E.m_E41+M.m_E.m_E41, m_E.m_E42+M.m_E.m_E42, m_E.m_E43+M.m_E.m_E43, m_E.m_E44+M.m_E.m_E44));
	};

	inline void operator += (Matrix & M)
	{
		m_E.m_E11+=M.m_E.m_E11; m_E.m_E12+=M.m_E.m_E12; m_E.m_E13+=M.m_E.m_E13; m_E.m_E14+=M.m_E.m_E14;
		m_E.m_E21+=M.m_E.m_E21; m_E.m_E22+=M.m_E.m_E22; m_E.m_E23+=M.m_E.m_E23; m_E.m_E24+=M.m_E.m_E24;
		m_E.m_E31+=M.m_E.m_E31; m_E.m_E32+=M.m_E.m_E32; m_E.m_E33+=M.m_E.m_E33; m_E.m_E34+=M.m_E.m_E34;
		m_E.m_E41+=M.m_E.m_E41; m_E.m_E42+=M.m_E.m_E42; m_E.m_E43+=M.m_E.m_E43; m_E.m_E44+=M.m_E.m_E44;
	};

	inline Matrix operator - (Matrix & M)
	{
		return(Matrix(
			m_E.m_E11-M.m_E.m_E11, m_E.m_E12-M.m_E.m_E12, m_E.m_E13-M.m_E.m_E13, m_E.m_E14-M.m_E.m_E14,
			m_E.m_E21-M.m_E.m_E21, m_E.m_E22-M.m_E.m_E22, m_E.m_E23-M.m_E.m_E23, m_E.m_E24-M.m_E.m_E24,
			m_E.m_E31-M.m_E.m_E31, m_E.m_E32-M.m_E.m_E32, m_E.m_E33-M.m_E.m_E33, m_E.m_E34-M.m_E.m_E34,
			m_E.m_E41-M.m_E.m_E41, m_E.m_E42-M.m_E.m_E42, m_E.m_E43-M.m_E.m_E43, m_E.m_E44-M.m_E.m_E44));
	};

	inline void operator -= (Matrix & M)
	{
		m_E.m_E11-=M.m_E.m_E11; m_E.m_E12-=M.m_E.m_E12; m_E.m_E13-=M.m_E.m_E13; m_E.m_E14-=M.m_E.m_E14;
		m_E.m_E21-=M.m_E.m_E21; m_E.m_E22-=M.m_E.m_E22; m_E.m_E23-=M.m_E.m_E23; m_E.m_E24-=M.m_E.m_E24;
		m_E.m_E31-=M.m_E.m_E31; m_E.m_E32-=M.m_E.m_E32; m_E.m_E33-=M.m_E.m_E33; m_E.m_E34-=M.m_E.m_E34;
		m_E.m_E41-=M.m_E.m_E41; m_E.m_E42-=M.m_E.m_E42; m_E.m_E43-=M.m_E.m_E43; m_E.m_E44-=M.m_E.m_E44;
	};

	inline Matrix operator * (const Matrix & M) const
	{
		Matrix Ret;

		for (unsigned int i = 0 ; i < 4 ; i++) {
			for (unsigned int j = 0 ; j < 4 ; j++) {
				Ret.mMat[i][j] = mMat[i][0] * M.mMat[0][j] +
								 mMat[i][1] * M.mMat[1][j] +
								 mMat[i][2] * M.mMat[2][j] +
								 mMat[i][3] * M.mMat[3][j];
			}
		}

		return Ret;
// 		return(Matrix(
// 			m_E.m_E11*M.m_E.m_E11 + m_E.m_E12*M.m_E.m_E21 + m_E.m_E13*M.m_E.m_E31 + m_E.m_E14*M.m_E.m_E41,
// 			m_E.m_E11*M.m_E.m_E12 + m_E.m_E12*M.m_E.m_E22 + m_E.m_E13*M.m_E.m_E32 + m_E.m_E14*M.m_E.m_E42,
// 			m_E.m_E11*M.m_E.m_E13 + m_E.m_E12*M.m_E.m_E23 + m_E.m_E13*M.m_E.m_E33 + m_E.m_E14*M.m_E.m_E43,
// 			m_E.m_E11*M.m_E.m_E14 + m_E.m_E12*M.m_E.m_E24 + m_E.m_E13*M.m_E.m_E34 + m_E.m_E14*M.m_E.m_E44,
// 			m_E.m_E21*M.m_E.m_E11 + m_E.m_E22*M.m_E.m_E21 + m_E.m_E23*M.m_E.m_E31 + m_E.m_E24*M.m_E.m_E41,
// 			m_E.m_E21*M.m_E.m_E12 + m_E.m_E22*M.m_E.m_E22 + m_E.m_E23*M.m_E.m_E32 + m_E.m_E24*M.m_E.m_E42,
// 			m_E.m_E21*M.m_E.m_E13 + m_E.m_E22*M.m_E.m_E23 + m_E.m_E23*M.m_E.m_E33 + m_E.m_E24*M.m_E.m_E43,
// 			m_E.m_E21*M.m_E.m_E14 + m_E.m_E22*M.m_E.m_E24 + m_E.m_E23*M.m_E.m_E34 + m_E.m_E24*M.m_E.m_E44,
// 			m_E.m_E31*M.m_E.m_E11 + m_E.m_E32*M.m_E.m_E21 + m_E.m_E33*M.m_E.m_E31 + m_E.m_E34*M.m_E.m_E41,
// 			m_E.m_E31*M.m_E.m_E12 + m_E.m_E32*M.m_E.m_E22 + m_E.m_E33*M.m_E.m_E32 + m_E.m_E34*M.m_E.m_E42,
// 			m_E.m_E31*M.m_E.m_E13 + m_E.m_E32*M.m_E.m_E23 + m_E.m_E33*M.m_E.m_E33 + m_E.m_E34*M.m_E.m_E43,
// 			m_E.m_E31*M.m_E.m_E14 + m_E.m_E32*M.m_E.m_E24 + m_E.m_E33*M.m_E.m_E34 + m_E.m_E34*M.m_E.m_E44,
// 			m_E.m_E41*M.m_E.m_E11 + m_E.m_E42*M.m_E.m_E21 + m_E.m_E43*M.m_E.m_E31 + m_E.m_E44*M.m_E.m_E41,
// 			m_E.m_E41*M.m_E.m_E12 + m_E.m_E42*M.m_E.m_E22 + m_E.m_E43*M.m_E.m_E32 + m_E.m_E44*M.m_E.m_E42,
// 			m_E.m_E41*M.m_E.m_E13 + m_E.m_E42*M.m_E.m_E23 + m_E.m_E43*M.m_E.m_E33 + m_E.m_E44*M.m_E.m_E43,
// 			m_E.m_E41*M.m_E.m_E14 + m_E.m_E42*M.m_E.m_E24 + m_E.m_E43*M.m_E.m_E34 + m_E.m_E44*M.m_E.m_E44));
	};

	inline void operator *= (Matrix & M)
	{
		float T11 = m_E.m_E11*M.m_E.m_E11 + m_E.m_E12*M.m_E.m_E21 + m_E.m_E13*M.m_E.m_E31 + m_E.m_E14*M.m_E.m_E41;
		float T12 = m_E.m_E11*M.m_E.m_E12 + m_E.m_E12*M.m_E.m_E22 + m_E.m_E13*M.m_E.m_E32 + m_E.m_E14*M.m_E.m_E42;
		float T13 = m_E.m_E11*M.m_E.m_E13 + m_E.m_E12*M.m_E.m_E23 + m_E.m_E13*M.m_E.m_E33 + m_E.m_E14*M.m_E.m_E43;
		float T14 = m_E.m_E11*M.m_E.m_E14 + m_E.m_E12*M.m_E.m_E24 + m_E.m_E13*M.m_E.m_E34 + m_E.m_E14*M.m_E.m_E44;
		float T21 = m_E.m_E21*M.m_E.m_E11 + m_E.m_E22*M.m_E.m_E21 + m_E.m_E23*M.m_E.m_E31 + m_E.m_E24*M.m_E.m_E41;
		float T22 = m_E.m_E21*M.m_E.m_E12 + m_E.m_E22*M.m_E.m_E22 + m_E.m_E23*M.m_E.m_E32 + m_E.m_E24*M.m_E.m_E42;
		float T23 = m_E.m_E21*M.m_E.m_E13 + m_E.m_E22*M.m_E.m_E23 + m_E.m_E23*M.m_E.m_E33 + m_E.m_E24*M.m_E.m_E43;
		float T24 = m_E.m_E21*M.m_E.m_E14 + m_E.m_E22*M.m_E.m_E24 + m_E.m_E23*M.m_E.m_E34 + m_E.m_E24*M.m_E.m_E44;
		float T31 = m_E.m_E31*M.m_E.m_E11 + m_E.m_E32*M.m_E.m_E21 + m_E.m_E33*M.m_E.m_E31 + m_E.m_E34*M.m_E.m_E41;
		float T32 = m_E.m_E31*M.m_E.m_E12 + m_E.m_E32*M.m_E.m_E22 + m_E.m_E33*M.m_E.m_E32 + m_E.m_E34*M.m_E.m_E42;
		float T33 = m_E.m_E31*M.m_E.m_E13 + m_E.m_E32*M.m_E.m_E23 + m_E.m_E33*M.m_E.m_E33 + m_E.m_E34*M.m_E.m_E43;
		float T34 = m_E.m_E31*M.m_E.m_E14 + m_E.m_E32*M.m_E.m_E24 + m_E.m_E33*M.m_E.m_E34 + m_E.m_E34*M.m_E.m_E44;
		float T41 = m_E.m_E41*M.m_E.m_E11 + m_E.m_E42*M.m_E.m_E21 + m_E.m_E43*M.m_E.m_E31 + m_E.m_E44*M.m_E.m_E41;
		float T42 = m_E.m_E41*M.m_E.m_E12 + m_E.m_E42*M.m_E.m_E22 + m_E.m_E43*M.m_E.m_E32 + m_E.m_E44*M.m_E.m_E42;
		float T43 = m_E.m_E41*M.m_E.m_E13 + m_E.m_E42*M.m_E.m_E23 + m_E.m_E43*M.m_E.m_E33 + m_E.m_E44*M.m_E.m_E43;
		float T44 = m_E.m_E41*M.m_E.m_E14 + m_E.m_E42*M.m_E.m_E24 + m_E.m_E43*M.m_E.m_E34 + m_E.m_E44*M.m_E.m_E44;

		m_E.m_E11 = T11; m_E.m_E12 = T12; m_E.m_E13 = T13; m_E.m_E14 = T14;
		m_E.m_E21 = T21; m_E.m_E22 = T22; m_E.m_E23 = T23; m_E.m_E24 = T24;
		m_E.m_E31 = T31; m_E.m_E32 = T32; m_E.m_E33 = T33; m_E.m_E34 = T34;
		m_E.m_E41 = T41; m_E.m_E42 = T42; m_E.m_E43 = T43; m_E.m_E44 = T44;
	};

	inline Matrix operator * (float S)
	{
		return(Matrix(
			m_E.m_E11*S, m_E.m_E12*S, m_E.m_E13*S, m_E.m_E14*S,
			m_E.m_E21*S, m_E.m_E22*S, m_E.m_E23*S, m_E.m_E24*S,
			m_E.m_E31*S, m_E.m_E32*S, m_E.m_E33*S, m_E.m_E34*S,
			m_E.m_E41*S, m_E.m_E42*S, m_E.m_E43*S, m_E.m_E44*S));
	};

	inline Matrix operator *= (float S)
	{
		return(Matrix(
			m_E.m_E11*=S, m_E.m_E12*=S, m_E.m_E13*=S, m_E.m_E14*=S,
			m_E.m_E21*=S, m_E.m_E22*=S, m_E.m_E23*=S, m_E.m_E24*=S,
			m_E.m_E31*=S, m_E.m_E32*=S, m_E.m_E33*=S, m_E.m_E34*=S,
			m_E.m_E41*=S, m_E.m_E42*=S, m_E.m_E43*=S, m_E.m_E44*=S));
	};

	inline Matrix operator / (float S)
	{
		return(Matrix(
			m_E.m_E11/S, m_E.m_E12/S, m_E.m_E13/S, m_E.m_E14/S,
			m_E.m_E21/S, m_E.m_E22/S, m_E.m_E23/S, m_E.m_E24/S,
			m_E.m_E31/S, m_E.m_E32/S, m_E.m_E33/S, m_E.m_E34/S,
			m_E.m_E41/S, m_E.m_E42/S, m_E.m_E43/S, m_E.m_E44/S));
	};

	inline Matrix operator /= (float S)
	{
		return(Matrix(
			m_E.m_E11/=S, m_E.m_E12/=S, m_E.m_E13/=S, m_E.m_E14/=S,
			m_E.m_E21/=S, m_E.m_E22/=S, m_E.m_E23/=S, m_E.m_E24/=S,
			m_E.m_E31/=S, m_E.m_E32/=S, m_E.m_E33/=S, m_E.m_E34/=S,
			m_E.m_E41/=S, m_E.m_E42/=S, m_E.m_E43/=S, m_E.m_E44/=S));
	};

// 	inline float& operator()(uint aRow, uint aCol)
// 	{
// 		return mMat[aRow][0];
// 	}

	inline void operator = (const Matrix& M)
	{
		m_E.m_E11 = M.m_E.m_E11; m_E.m_E12 = M.m_E.m_E12; m_E.m_E13 = M.m_E.m_E13; m_E.m_E14 = M.m_E.m_E14;
		m_E.m_E21 = M.m_E.m_E21; m_E.m_E22 = M.m_E.m_E22; m_E.m_E23 = M.m_E.m_E23; m_E.m_E24 = M.m_E.m_E24;
		m_E.m_E31 = M.m_E.m_E31; m_E.m_E32 = M.m_E.m_E32; m_E.m_E33 = M.m_E.m_E33; m_E.m_E34 = M.m_E.m_E34;
		m_E.m_E41 = M.m_E.m_E41; m_E.m_E42 = M.m_E.m_E42; m_E.m_E43 = M.m_E.m_E43; m_E.m_E44 = M.m_E.m_E44;
	};

	inline bool operator == (const Matrix& M)
	{
		return(
			m_E.m_E11 == M.m_E.m_E11 && m_E.m_E12 == M.m_E.m_E12 && m_E.m_E13 == M.m_E.m_E13 && m_E.m_E14 == M.m_E.m_E14 &&
			m_E.m_E21 == M.m_E.m_E21 && m_E.m_E22 == M.m_E.m_E22 && m_E.m_E23 == M.m_E.m_E23 && m_E.m_E24 == M.m_E.m_E24 &&
			m_E.m_E31 == M.m_E.m_E31 && m_E.m_E32 == M.m_E.m_E32 && m_E.m_E33 == M.m_E.m_E33 && m_E.m_E34 == M.m_E.m_E34 &&
			m_E.m_E41 == M.m_E.m_E41 && m_E.m_E42 == M.m_E.m_E42 && m_E.m_E43 == M.m_E.m_E43 && m_E.m_E44 == M.m_E.m_E44);
	};

	inline Vec3 operator * (const Vec3& V) const
	{
		return(Vec3(
			m_E.m_E11*V.m.x + m_E.m_E21*V.m.y + m_E.m_E31*V.m.z,
			m_E.m_E12*V.m.x + m_E.m_E22*V.m.y + m_E.m_E32*V.m.z,
			m_E.m_E13*V.m.x + m_E.m_E23*V.m.y + m_E.m_E33*V.m.z));
	};

	/*
	 *	use operator [] (index from 0 to 16)
	 */
	inline float& operator() (const int arg)
	{
		if (arg < 16)
		{
			return mLineMat[arg];
		}
		else
		{
			Output::Print("Error: matrix  operator[] Argument invalid\n");
			return mLineMat[0];
		}
	}

	/*
	 * use like operator [][] : idex from 0 to 4
	 */
	inline float* operator[] (const int arg1)
	{
		if (arg1 < 4 )
		{
			return mMat[arg1];
		}
		else
		{
			Output::Print("Error: matrix  operator[] Argument invalid\n");
			return NULL;
		}
	}

	inline static Matrix NewScaleMatrix(const Vec3& aVec)
	{
		return NewScaleMatrix(aVec.X(),aVec.Y(),aVec.Z());
	}
	
	inline static Matrix NewScaleMatrix(float X, float Y, float Z)
	{
		return(
			Matrix(X, 0, 0, 0,
			       0, Y, 0, 0,
			       0, 0, Z, 0,
			       0, 0, 0, 1)
			       );
	};

	inline static Matrix NewTranslationMatrix(const Vec3& aVec)
	{
		return NewTranslationMatrix(aVec.X(), aVec.Y(),aVec.Z());
	}

	inline static Matrix NewTranslationMatrix(float X, float Y, float Z)
	{
		return(
			Matrix( 1, 0, 0, X,
					0, 1, 0, Y,
					0, 0, 1, Z,
					0, 0, 0, 1)
					);
	};

	inline static Matrix NewRotationMatrixAxisX(float Angle)
	{
		float Cos = cosf(Angle);
		float Sin = sinf(Angle);

		return(
			Matrix( 1,  0,   0,   0,
					0,  Cos,-Sin, 0,
					0,  Sin, Cos, 0,
					0,  0,   0,   1));
	};

	inline static Matrix NewRotationMatrixAxisY(float Angle)
	{
		float Cos = cosf(Angle);
		float Sin = sinf(Angle);

		return(
			Matrix(Cos, 0, -Sin, 0,
					0,   1,  0,   0,
					Sin, 0,  Cos, 0,
					0,   0,  0,   1)
					);
	};

	inline static Matrix NewRotationMatrixAxisZ(float Angle)
	{
		float Cos = cosf(Angle);
		float Sin = sinf(Angle);

		return(
			Matrix( Cos, -Sin, 0, 0,
					Sin,  Cos, 0, 0,
					0,    0,   1, 0,
					0,    0,   0, 1)
					);
	};


	inline Matrix Transpose() const 
	{
		return(Matrix(
			m_E.m_E11, m_E.m_E21, m_E.m_E31, m_E.m_E41,
			m_E.m_E12, m_E.m_E22, m_E.m_E32, m_E.m_E42,
			m_E.m_E13, m_E.m_E23, m_E.m_E33, m_E.m_E43,
			m_E.m_E14, m_E.m_E24, m_E.m_E34, m_E.m_E44));
	};

	inline void SelfTranspose()
	{
		float E12 = m_E.m_E21; float E13 = m_E.m_E31; float E14 = m_E.m_E41;
		float E21 = m_E.m_E12; float E23 = m_E.m_E32; float E24 = m_E.m_E42;
		float E31 = m_E.m_E13; float E32 = m_E.m_E23; float E34 = m_E.m_E43;
		float E41 = m_E.m_E14; float E42 = m_E.m_E24; float E43 = m_E.m_E34;

		m_E.m_E12 = E12; m_E.m_E13 = E13; m_E.m_E14 = E14;
		m_E.m_E21 = E21; m_E.m_E23 = E23; m_E.m_E24 = E24;
		m_E.m_E31 = E31; m_E.m_E32 = E32; m_E.m_E34 = E34;
		m_E.m_E41 = E41; m_E.m_E42 = E42; m_E.m_E43 = E43;
	};

	inline void Multiply(Matrix & M)
	{
		float T11 = m_E.m_E11*M.m_E.m_E11 + m_E.m_E12*M.m_E.m_E21 + m_E.m_E13*M.m_E.m_E31 + m_E.m_E14*M.m_E.m_E41;
		float T12 = m_E.m_E11*M.m_E.m_E12 + m_E.m_E12*M.m_E.m_E22 + m_E.m_E13*M.m_E.m_E32 + m_E.m_E14*M.m_E.m_E42;
		float T13 = m_E.m_E11*M.m_E.m_E13 + m_E.m_E12*M.m_E.m_E23 + m_E.m_E13*M.m_E.m_E33 + m_E.m_E14*M.m_E.m_E43;
		float T14 = m_E.m_E11*M.m_E.m_E14 + m_E.m_E12*M.m_E.m_E24 + m_E.m_E13*M.m_E.m_E34 + m_E.m_E14*M.m_E.m_E44;
		float T21 = m_E.m_E21*M.m_E.m_E11 + m_E.m_E22*M.m_E.m_E21 + m_E.m_E23*M.m_E.m_E31 + m_E.m_E24*M.m_E.m_E41;
		float T22 = m_E.m_E21*M.m_E.m_E12 + m_E.m_E22*M.m_E.m_E22 + m_E.m_E23*M.m_E.m_E32 + m_E.m_E24*M.m_E.m_E42;
		float T23 = m_E.m_E21*M.m_E.m_E13 + m_E.m_E22*M.m_E.m_E23 + m_E.m_E23*M.m_E.m_E33 + m_E.m_E24*M.m_E.m_E43;
		float T24 = m_E.m_E21*M.m_E.m_E14 + m_E.m_E22*M.m_E.m_E24 + m_E.m_E23*M.m_E.m_E34 + m_E.m_E24*M.m_E.m_E44;
		float T31 = m_E.m_E31*M.m_E.m_E11 + m_E.m_E32*M.m_E.m_E21 + m_E.m_E33*M.m_E.m_E31 + m_E.m_E34*M.m_E.m_E41;
		float T32 = m_E.m_E31*M.m_E.m_E12 + m_E.m_E32*M.m_E.m_E22 + m_E.m_E33*M.m_E.m_E32 + m_E.m_E34*M.m_E.m_E42;
		float T33 = m_E.m_E31*M.m_E.m_E13 + m_E.m_E32*M.m_E.m_E23 + m_E.m_E33*M.m_E.m_E33 + m_E.m_E34*M.m_E.m_E43;
		float T34 = m_E.m_E31*M.m_E.m_E14 + m_E.m_E32*M.m_E.m_E24 + m_E.m_E33*M.m_E.m_E34 + m_E.m_E34*M.m_E.m_E44;
		float T41 = m_E.m_E41*M.m_E.m_E11 + m_E.m_E42*M.m_E.m_E21 + m_E.m_E43*M.m_E.m_E31 + m_E.m_E44*M.m_E.m_E41;
		float T42 = m_E.m_E41*M.m_E.m_E12 + m_E.m_E42*M.m_E.m_E22 + m_E.m_E43*M.m_E.m_E32 + m_E.m_E44*M.m_E.m_E42;
		float T43 = m_E.m_E41*M.m_E.m_E13 + m_E.m_E42*M.m_E.m_E23 + m_E.m_E43*M.m_E.m_E33 + m_E.m_E44*M.m_E.m_E43;
		float T44 = m_E.m_E41*M.m_E.m_E14 + m_E.m_E42*M.m_E.m_E24 + m_E.m_E43*M.m_E.m_E34 + m_E.m_E44*M.m_E.m_E44;

		m_E.m_E11 = T11; m_E.m_E12 = T12; m_E.m_E13 = T13; m_E.m_E14 = T14;
		m_E.m_E21 = T21; m_E.m_E22 = T22; m_E.m_E23 = T23; m_E.m_E24 = T24;
		m_E.m_E31 = T31; m_E.m_E32 = T32; m_E.m_E33 = T33; m_E.m_E34 = T34;
		m_E.m_E41 = T41; m_E.m_E42 = T42; m_E.m_E43 = T43; m_E.m_E44 = T44;
	};

	inline Matrix RotateX(float Angle)
	{
		return(*this * NewRotationMatrixAxisX(Angle));
	};

	inline Matrix RotateY(float Angle)
	{
		return(*this * NewRotationMatrixAxisY(Angle));
	};

	inline Matrix RotateZ(float Angle)
	{
		return(*this * NewRotationMatrixAxisZ(Angle));
	};

	inline void SelfRotateX(float Angle)
	{
		Matrix m = NewRotationMatrixAxisX(Angle);
		this->Multiply(m);
	};

	inline void SelfRotateY(float Angle)
	{
		Matrix m = NewRotationMatrixAxisY(Angle);
		this->Multiply(m);
	};

	inline void SelfRotateZ(float Angle)
	{
		Matrix m = NewRotationMatrixAxisZ(Angle);
		this->Multiply(m);
	};

	inline Matrix Translate(float X, float Y, float Z) const
	{
		return(*this * NewTranslationMatrix(X, Y, Z));
	};

	inline void SelfTranslate(float X, float Y, float Z)
	{
// 		Matrix OO = NewTranslationMatrix(X, Y, Z);
// 		this->Multiply(OO);

		float T11 = m_E.m_E11 * 1.0f + m_E.m_E12 * 0.0f + m_E.m_E13*0.0f + m_E.m_E14 * 0.0f;
		float T12 = m_E.m_E11 * 0.0f + m_E.m_E12 * 1.0f + m_E.m_E13*0.0f + m_E.m_E14 * 0.0f;
		float T13 = m_E.m_E11 * 0.0f + m_E.m_E12 * 0.0f + m_E.m_E13*1.0f + m_E.m_E14 * 0.0f;
		float T14 = m_E.m_E11 * X    + m_E.m_E12 *  Y   + m_E.m_E13* Z   + m_E.m_E14 * 1.0f;
		float T21 = m_E.m_E21 * 1.0f + m_E.m_E22 * 0.0f + m_E.m_E23*0.0f + m_E.m_E24 * 0.0f;
		float T22 = m_E.m_E21 * 0.0f + m_E.m_E22 * 1.0f + m_E.m_E23*0.0f + m_E.m_E24 * 0.0f;
		float T23 = m_E.m_E21 * 0.0f + m_E.m_E22 * 0.0f + m_E.m_E23*1.0f + m_E.m_E24 * 0.0f;
		float T24 = m_E.m_E21 * X    + m_E.m_E22 *  Y   + m_E.m_E23* Z   + m_E.m_E24 * 1.0f;
		float T31 = m_E.m_E31 * 1.0f + m_E.m_E32 * 0.0f + m_E.m_E33*0.0f + m_E.m_E34 * 0.0f;
		float T32 = m_E.m_E31 * 0.0f + m_E.m_E32 * 1.0f + m_E.m_E33*0.0f + m_E.m_E34 * 0.0f;
		float T33 = m_E.m_E31 * 0.0f + m_E.m_E32 * 0.0f + m_E.m_E33*1.0f + m_E.m_E34 * 0.0f;
		float T34 = m_E.m_E31 * X    + m_E.m_E32 *  Y   + m_E.m_E33* Z   + m_E.m_E34 * 1.0f;
		float T41 = m_E.m_E41 * 1.0f + m_E.m_E42 * 0.0f + m_E.m_E43*0.0f + m_E.m_E44 * 0.0f;
		float T42 = m_E.m_E41 * 0.0f + m_E.m_E42 * 1.0f + m_E.m_E43*0.0f + m_E.m_E44 * 0.0f;
		float T43 = m_E.m_E41 * 0.0f + m_E.m_E42 * 0.0f + m_E.m_E43*1.0f + m_E.m_E44 * 0.0f;
		float T44 = m_E.m_E41 * X    + m_E.m_E42 *  Y   + m_E.m_E43* Z   + m_E.m_E44 * 1.0f;

		m_E.m_E11 = T11; m_E.m_E12 = T12; m_E.m_E13 = T13; m_E.m_E14 = T14;
		m_E.m_E21 = T21; m_E.m_E22 = T22; m_E.m_E23 = T23; m_E.m_E24 = T24;
		m_E.m_E31 = T31; m_E.m_E32 = T32; m_E.m_E33 = T33; m_E.m_E34 = T34;
		m_E.m_E41 = T41; m_E.m_E42 = T42; m_E.m_E43 = T43; m_E.m_E44 = T44;
	};

	inline Matrix Scale(float X, float Y, float Z) const
	{
		return(*this * NewScaleMatrix(X, Y, Z));
	};

	inline void SelfScale(float X, float Y, float Z)
	{
		Matrix m = NewScaleMatrix(X, Y, Z);
		this->Multiply(m);
	};

	inline void SelfScale(const Vec3& aVec)
	{
		Matrix m = NewScaleMatrix(aVec.X(), aVec.Y(), aVec.Z());
		this->Multiply(m);
	};

	inline void Reset()
	{
		SetIdentity();
	};

	inline void InitFromEulerAngle(float X, float Y, float Z)
	{
		float A  = cosf(X);
		float B  = sinf(X);
		float C  = cosf(Y);
		float D  = sinf(Y);
		float E  = cosf(Z);
		float F  = sinf(Z);

		float AD = A * D;
		float BD = B * D;

		m_E.m_E11    =   C * E;
		m_E.m_E12    =  -C * F;
		m_E.m_E13    =  -D;

		m_E.m_E21    = -BD * E + A * F;
		m_E.m_E23    =  BD * F + A * E;
		m_E.m_E24    =  -B * C;

		m_E.m_E31    =  AD * E + B * F;
		m_E.m_E32    = -AD * F + B * E;
		m_E.m_E33    =   A * C;

		m_E.m_E14    =  m_E.m_E24 = m_E.m_E34 = m_E.m_E41  = m_E.m_E42 = m_E.m_E43 = 0.0f;
		m_E.m_E44    =  1.0f;
	};

	inline static Matrix FromEulerAngle(float X, float Y, float Z)
	{
		Matrix m;

		float A  = cosf(X);
		float B  = sinf(X);
		float C  = cosf(Y);
		float D  = sinf(Y);
		float E  = cosf(Z);
		float F  = sinf(Z);

		float AD = A * D;
		float BD = B * D;

		m.m_E.m_E11    =   C * E;
		m.m_E.m_E12    =  -C * F;
		m.m_E.m_E13    =  -D;

		m.m_E.m_E21    = -BD * E + A * F;
		m.m_E.m_E23    =  BD * F + A * E;
		m.m_E.m_E24    =  -B * C;

		m.m_E.m_E31    =  AD * E + B * F;
		m.m_E.m_E32    = -AD * F + B * E;
		m.m_E.m_E33    =   A * C;

		m.m_E.m_E14    =  m.m_E.m_E24 = m.m_E.m_E34 = m.m_E.m_E41  = m.m_E.m_E42 = m.m_E.m_E43 = 0.0f;
		m.m_E.m_E44    =  1.0f;

		return m;
	};

	float * ConvertToArray();

	inline Vec3 ExtractEulerAngle() const;
	
	~Matrix(void){};

	inline void SetTransformation(Vec3& aTranslation, Vec3& aScale, Vec3& aRotation);
};

Vec3 Matrix::ExtractEulerAngle() const 
{
	Vec3 v;

	float C,D;
	float tmpY, tmpX;

	v.m.y = D = -asin(m_E.m_E13);
	C   =  cos(v.m.y);

	if (ABS(C) > 0.005f)
	{
		tmpX =  m_E.m_E33 / C;
		tmpY = -m_E.m_E23 / C;

		v.m.x  = atan2(tmpY, tmpX);

		tmpX =  m_E.m_E11 / C;
		tmpY = -m_E.m_E12 / C;

		v.m.z  = atan2(tmpY, tmpX);
	}
	else
	{
		// Gimball lock has occurred
		v.m.x  = 0.0f;

		tmpX = m_E.m_E22;
		tmpY = m_E.m_E21;

		v.m.z  = atan2( tmpY, tmpX );
	}

	// clamp all angles to range
	v.m.x = (v.m.x < 0 ? 0.0f : ( v.m.x > 6.28319f ? 6.28319f : v.m.x));
	v.m.y = (v.m.y < 0 ? 0.0f : ( v.m.y > 6.28319f ? 6.28319f : v.m.y));
	v.m.z = (v.m.z < 0 ? 0.0f : ( v.m.z > 6.28319f ? 6.28319f : v.m.z));

	return v;
}

inline void Matrix::SetTransformation(Vec3& aTranslation, Vec3& aScale, Vec3& aRotation)
{
	Matrix mRotationX, mRotationY, mRotationZ;

	Reset();

	mRotationX = NewRotationMatrixAxisX(aRotation.X());
	mRotationY = NewRotationMatrixAxisY(aRotation.Y());
	mRotationZ = NewRotationMatrixAxisZ(aRotation.Z());
	
	*this = (NewTranslationMatrix(aTranslation.X(), aTranslation.Y(), aTranslation.Z()) 
			* (mRotationZ * mRotationY * mRotationX)
			* NewScaleMatrix(aScale.X(), aScale.Y(), aScale.Z()));
}
#endif